#! /usr/bin/env python3
import rospy
import sys#在终端输入命令时可以添加参数
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf_conversions

turtle_name=""


def doPose(pose):
        #创建发布广播者
        broadcaster=tf2_ros.TransformBroadcaster()
        #创建相对坐标系
        tf=TransformStamped()
        tf.header.frame_id="world"
        tf.header.stamp=rospy.Time().now()
        #将小乌龟的坐标导入的这个相对坐标系中
        tf.child_frame_id=turtle_name
        tf.transform.translation.x=pose.x
        tf.transform.translation.y=pose.y
        qtn=tf_conversions.transformations.quaternion_from_euler(0,0,0)
        tf.transform.rotation.x=qtn[0]
        tf.transform.rotation.y=qtn[1]
        tf.transform.rotation.z=qtn[2]
        tf.transform.rotation.w=qtn[3]
        #发送广播
        print("turtle pose is:x:%s,y:%s",tf.transform.translation.x,tf.transform.translation.y)
        broadcaster.sendTransform(tf)




if __name__=="__main__":
    rospy.init_node("sub_tf_p")
    
    #从终端获取输入
    rospy.loginfo("----------%d",len(sys.argv))
    if len(sys.argv)<2:
        rospy.loginfo("please give a turtle's namespace")
    else:
      turtle_name=sys.argv[1]
      rospy.loginfo("-----------turtle :%s",turtle_name)
      #订阅小乌龟的消息
      rospy.Subscriber(turtle_name+"/pose",Pose,doPose)#turtle_name+"pose":字符串的拼接,表示话题路径
    rospy.spin()